!> 刚体粒子集模块
module sph_rigid_body
    use sph_kinds, only: rk
    use sph_particle, only: particle_t
    use sph_env, only: dim
    !> 刚体粒子集数据结构
    type rigid_body
        integer :: istart, iend  !! 刚体粒子集的索引
        real(rk) :: mass(2)  !! 刚体质量、转动惯量
        real(rk) :: loc(2)  !! 刚体质心
        real(rk) :: vel(2)  !! 刚体速度
        real(rk) :: ang_vel  !! 刚体角速度
        real(rk) :: acc(2)  !! 刚体加速度
        real(rk) :: ang_acc  !! 刚体角加速度
        real(rk) :: vmin(2)  !! 临时变量
        real(rk) :: avmid  !! 临时变量
    contains
        procedure :: setup, get_acc, update_particle_init, update_particle_step
    end type rigid_body
contains
    !> 初始化刚体
    pure subroutine setup(this, particle)
        class(rigid_body), intent(inout) :: this  !! 刚体粒子集
        type(particle_t), intent(in) :: particle  !! 区域
        integer :: i
        associate (mass => particle%mass(this%istart:this%iend), &
                   loc => particle%loc(:, this%istart:this%iend))
            this%mass(1) = sum(mass)
            do i = 1, dim
                this%loc(i) = sum(mass*loc(i, :))/this%mass(1)
            end do
            this%mass(2) = sum(mass*((loc(1, :) - this%loc(1))**2 + &
                                     (loc(2, :) - this%loc(2))**2))
        end associate
        this%vel = 0.0_rk
    end subroutine setup
    !> 获取刚体受力
    pure subroutine get_acc(this, particles)
        class(rigid_body), intent(inout) :: this  !! 刚体粒子集
        type(particle_t), intent(in) :: particles  !! 区域
        integer :: i
        real(rk) :: force(2), torque
        force = 0.0_rk
        torque = 0.0_rk
        do i = this%istart, this%iend
            force = force + particles%acc(:, i)*particles%mass(i)
            torque = torque + cross_product_local(particles%loc(:, i) - this%loc(:), &
                                                  particles%acc(:, i)*particles%mass(i))
        end do
        this%acc = force/this%mass(1)
        this%ang_acc = torque/this%mass(2)
    end subroutine get_acc
    !> 简单叉乘
    pure function cross_product_local(a, b) result(c)
        real(rk), intent(in), dimension(2) :: a, b
        real(rk) :: c
        c = a(1)*b(2) - a(2)*b(1)
    end function cross_product_local
    !> 根据刚体受力，求解加速度，更新刚体的速度和位置
    pure subroutine update_particle_init(this, particles, dt)
        class(rigid_body), intent(inout) :: this  !! 刚体粒子集
        type(particle_t), intent(inout) :: particles  !! 区域
        real(rk), intent(in) :: dt  !! 时间步长
        integer :: i
        associate (halfdt => dt/2)
            this%vel = this%vel + this%acc*halfdt
            this%loc = this%loc + this%vel*dt
            this%ang_vel = this%ang_vel + this%ang_acc*halfdt
            this%vmin = this%vel
            this%avmid = this%ang_vel
            this%vel = this%vel + this%acc*halfdt
            this%ang_vel = this%ang_vel + this%ang_acc*halfdt
        end associate
        ! 更新粒子
        do i = this%istart, this%iend
            particles%vel(:, i) = this%vmin + cross_product_local2( &
                                  this%avmid, &
                                  particles%loc(:, i) - this%loc(:))
            particles%loc(:, i) = particles%loc(:, i) + particles%vel(:, i)*dt
        end do
    end subroutine update_particle_init
    !> 更新粒子
    pure subroutine update_particle_step(this, particles, dt)
        class(rigid_body), intent(inout) :: this  !! 刚体粒子集
        type(particle_t), intent(inout) :: particles  !! 区域
        real(rk), intent(in) :: dt  !! 时间步长
        integer :: i
        associate (halfdt => dt/2)
            this%vel = this%vmin + this%acc*dt
            this%loc = this%loc + this%vel*dt
            this%ang_vel = this%avmid + this%ang_acc*dt
            this%vmin = this%vel
            this%avmid = this%ang_vel
            this%vel = this%vel + this%acc*halfdt
            this%ang_vel = this%ang_vel + this%ang_acc*halfdt
        end associate
        ! 更新粒子
        do i = this%istart, this%iend
            particles%vel(:, i) = this%vmin + cross_product_local2( &
                                  this%avmid, &
                                  particles%loc(:, i) - this%loc(:))
            particles%loc(:, i) = particles%loc(:, i) + particles%vel(:, i)*dt
        end do
    end subroutine update_particle_step
    !> 简单叉乘 2
    pure function cross_product_local2(a, b) result(c)
        real(rk), intent(in) :: a
        real(rk), intent(in), dimension(2) :: b
        real(rk), dimension(2) :: c
        c(1) = -a*b(2)
        c(2) = a*b(1)
    end function cross_product_local2
end module sph_rigid_body
